#include "Motor.h"

const short AAR = 1000;

void Motor_Control(MotorDirection direction, unsigned short speed) {
  unsigned short s;
  if (speed == 0) {
    LL_TIM_OC_SetCompareCH1(TIM10, AAR);
    LL_TIM_OC_SetCompareCH1(TIM11, AAR);
    LL_TIM_OC_SetCompareCH1(TIM9, AAR);
    LL_TIM_OC_SetCompareCH2(TIM9, AAR);
    return;
  }
  if (direction == MotorDirection_Backward) {
    LL_TIM_OC_SetCompareCH1(TIM11, 0);
    LL_TIM_OC_SetCompareCH2(TIM9, 0);
    s = speed % 101;
    LL_TIM_OC_SetCompareCH1(TIM10, AAR / 100 * s);
    LL_TIM_OC_SetCompareCH1(TIM9, AAR / 100 * s);
  } else {
    LL_TIM_OC_SetCompareCH1(TIM10, 0);
    LL_TIM_OC_SetCompareCH1(TIM9, 0);
    s = speed % 101;
    LL_TIM_OC_SetCompareCH1(TIM11, AAR / 100 * s);
    LL_TIM_OC_SetCompareCH2(TIM9, AAR / 100 * s);
  }
}

void Motor_Break(void) {
  LL_TIM_OC_SetCompareCH1(TIM10, 0);
  LL_TIM_OC_SetCompareCH1(TIM11, 0);
  LL_TIM_OC_SetCompareCH1(TIM9, 0);
  LL_TIM_OC_SetCompareCH2(TIM9, 0);
}

void Motor_ExampleWithoutLoop(void){
  Motor_Control(MotorDirection_Forward, 0);
  LL_mDelay(2000);
  Motor_Control(MotorDirection_Forward, 10);
  LL_mDelay(2000);
  Motor_Control(MotorDirection_Forward, 20);
  LL_mDelay(2000);
  Motor_Control(MotorDirection_Forward, 30);
  LL_mDelay(2000);
  Motor_Control(MotorDirection_Forward, 40);
  LL_mDelay(2000);
  Motor_Control(MotorDirection_Forward, 50);
  LL_mDelay(2000);
  Motor_Break();
  LL_mDelay(3000);
  Motor_Control(MotorDirection_Backward, 10);
  LL_mDelay(2000);
  Motor_Control(MotorDirection_Backward, 20);
  LL_mDelay(2000);
  Motor_Control(MotorDirection_Backward, 30);
  LL_mDelay(2000);
  Motor_Control(MotorDirection_Backward, 40);
  LL_mDelay(2000);
  Motor_Control(MotorDirection_Backward, 50);
  LL_mDelay(2000);
}
